#include "obstacle_detector/bbox_util.h"


namespace obstacle_detector
{
void get_bbox_velocity(int label,const Eigen::MatrixXf &bbox1,
	const Eigen::MatrixXf &bbox2,Eigen::MatrixXf &v)
{
	v=Eigen::MatrixXf::Zero(2,5);
	if (bbox1.sum()==0 || bbox2.sum()==0)
	{
		return;
	}

	Object obj1(bbox1);
	Object obj2(bbox2);
	float iou=bbox_iou(obj1,obj2);

	//8代表行人 行人前后两帧可能没有iou
	// printf ("iou: %d,%f\n",label,iou);
	if (label!=8 && iou<0.05) return;
	// std::cout<<bbox1<<std::endl;
	// std::cout<<bbox2<<std::endl;

	bool matched[4]={false,false,false,false};
	for (int i=0;i<4;i++)
	{
		float min_dis=10000;
		int min_index=-1;
		for (int j=0;j<4;j++)
		{
			if (matched[j]) continue;
			auto diff=bbox1.col(i)-bbox2.col(j);
			if (diff.norm()<min_dis)
			{
				min_dis=diff.norm();
				min_index=j;
			}
		}
		if (min_index==-1)
		{
			continue;
		}
		matched[min_index]=true;
		v.col(i)=bbox1.col(i)-bbox2.col(min_index);
	}

	v.col(4)=bbox1.rowwise().mean() - bbox2.rowwise().mean();
	for (int i=0;i<v.cols();i++)
	{
		if (fabs(v(0,i))>3 || fabs(v(1,i))>3)
		{
			v(0,i)=0;
			v(1,i)=0;
		}
	}

}

void bbox_nms(std::vector<Object> &obj)
{
	if (obj.size()==0) return;
	std::vector<Object> valid_obj;
	std::vector<bool> suspend(obj.size(),false);
	for (int i=0;i<obj.size()-1;i++)
	{
		if (suspend[i]) continue;
		int label1=obj[i].label;
		for (int j=i;j<obj.size();j++)
		{
			if (suspend[j]) continue;
			int label2=obj[j].label;
			if (label1!=label2) continue;
			float iou=bbox_iou(obj[i],obj[j]);
			if (iou>0.5)
			{
				suspend[j]=true;
				float area1=obj[i].global_dim[0]*obj[i].global_dim[1];
				float area2=obj[j].global_dim[0]*obj[j].global_dim[1];
				if (area2>area1) obj[i]=obj[j];
			}
		}
		// if (!overlap)
		// {
		valid_obj.push_back(obj[i]);
		// }
	}
	obj=valid_obj;
}


float bbox_iou(const Object &obj1,
	const Object &obj2)
{
	// Eigen::Matrix2f rotation1;
	// rotation1<< cos(obj1.global_yaw),sin(obj1.global_yaw),
	// 		-1*sin(obj1.global_yaw),cos(obj1.global_yaw);
	// Eigen::Vector2f center1=obj1.global_center;
	// printf ("bbox1 center: %f,%f\n",obj1.center(0),obj1.center(1));
	Eigen::Vector2f bbox1_dim=obj1.global_dim;
	Eigen::Vector2f bbox2_dim=obj2.global_dim;
	// printf("bbox1 dim: %f,%f\n",bbox1_dim(0),bbox1_dim(1));
	// printf("bbox2 dim: %f,%f\n",bbox2_dim(0),bbox2_dim(1));

	Eigen::MatrixXf bbox1_1(2,4);//第一个框在第一个框的局部坐标系下的坐标
	// //顺时针排列
	get_local_bbox(obj1,obj1,bbox1_1);
	// std::cout<<"bbox1_1: "<<std::endl;
	// std::cout<<bbox1_1<<std::endl;
	Eigen::MatrixXf bbox1_2; //第一个框在第二个框的局部坐标系下的坐标
	Eigen::MatrixXf bbox2_1; //第二个框在第一个框的局部坐标系下的坐标
	get_local_bbox(obj1,obj2,bbox2_1);
	// std::cout<<"bbox2_1: "<<std::endl;
	// std::cout<<bbox2_1<<std::endl;
	get_local_bbox(obj2,obj1,bbox1_2);
	// std::cout<<"bbox1_2: "<<std::endl;
	// std::cout<<bbox1_2<<std::endl;


	std::vector<Eigen::Vector2f> final_polygon;
	for (int i=0;i<4;i++)
	{
		Eigen::Vector2f p1=bbox2_1.col(i);
		Eigen::Vector2f p2=bbox2_1.col((i+1)%4);
		Eigen::Vector2f p5=bbox2_1.col((i+2)%4);

		if (p1(0)>-1*bbox1_dim[0]/2 && p1(0)<bbox1_dim[0]/2 && p1(1)>-1*bbox1_dim[1]/2 && p1(1)<bbox1_dim[1]/2)
		{
			add2polygon(final_polygon,p1);
		}

		// Eigen::Vector3f b1d=cross_product(p2-p1,p5-p1);//方向向量
		// printf ("b1d: %f,%f,%f\n",b1d(0),b1d(1),b1d(2));

		for (int j=0;j<4;j++) //被切割矩形
		{
			//寻找两条直线的交点
			Eigen::Vector2f p3=bbox1_1.col(j);
			Eigen::Vector2f p4=bbox1_1.col((j+1)%4);
			Eigen::Vector2f inter;

			Eigen::Vector2f p3_2=bbox1_2.col(j);
			if (p3_2(0)>=-1*bbox2_dim[0]/2 && p3_2(0)<=bbox2_dim[0]/2 && p3_2(1)>=-1*bbox2_dim[1]/2 && p3_2(1)<=bbox2_dim[1]/2)
			{
				// printf ("pt2 in bbox2\n");
				add2polygon(final_polygon,p3);
			}
			// printf ("check %d,%d\n",i,j);
			bool has_inter=get_inter(p1,p2,p3,p4,inter);
			if (!has_inter) continue;
			add2polygon(final_polygon,inter);

		}
	}
	// for (int i=0;i<final_polygon.size();i++)
	// {
	// 	printf ("final polygon %d: %f,%f\n",i,final_polygon[i](0),final_polygon[i](1));
	// }
	// printf ("final_polygon size: %d\n",final_polygon.size());
	float polygon_area=compute_polygon_area(final_polygon);
	float area1=bbox1_dim(0)*bbox1_dim(1);
	float area2=bbox2_dim(0)*bbox2_dim(1);
	float iou=polygon_area/(area1+area2-polygon_area);
	// printf ("iou: %f,%f,%f,%f\n",area1,area2,polygon_area,iou);

	// exit(-1);


	return iou;
}

//获取p1p2组成的线段和p3p4组成的线段的交点
//没有交点返回false
bool get_inter(const Eigen::Vector2f &p1,
	const Eigen::Vector2f &p2,const Eigen::Vector2f &p3,
	const Eigen::Vector2f &p4,Eigen::Vector2f &inter)
{
	Eigen::Vector2f v1=p3-p1;
	Eigen::Vector2f v2=p4-p1;
	Eigen::Vector2f v3=p3-p2;
	Eigen::Vector2f v4=p4-p2;
	float area_134=(v1(0)*v2(1)-v1(1)*v2(0))/2;
	float area_234=(v3(0)*v4(1)-v3(1)*v4(0))/2;
	float area_123=(v1(0)*v3(1)-v1(1)*v3(0))/2;
	float area_124=(v2(0)*v4(1)-v2(1)*v4(0))/2;
	if (area_134*area_234>0) return false;
	if (area_123*area_124>0) return false; 
	if (area_134==0 && area_234==0) return false;
	float ratio=fabs(area_134)/(fabs(area_134)+fabs(area_234));
	Eigen::Vector2f v5=p2-p1;
	inter=p1+ratio*v5;
	// printf ("line1: %f,%f,%f,%f\n",p1(0),p1(1),p2(0),p2(1));
	// printf ("line2: %f,%f,%f,%f\n",p3(0),p3(1),p4(0),p4(1));
	// printf ("inter: %f,%f\n",inter(0),inter(1));
	return true;
}

void add2polygon(std::vector<Eigen::Vector2f> &polygon,
	Eigen::Vector2f &point)
{
	if (polygon.size()==0) 
	{
		// printf ("add point 2 polygon: %f,%f\n",point(0),point(1));
		polygon.push_back(point);
	}
	for (int i=0;i<polygon.size();i++)
	{
		Eigen::Vector2f diff=point-polygon[i];
		if (diff.norm()<0.00001)
		{
			return;
		}
	}

	// printf ("add point 2 polygon: %f,%f\n",point(0),point(1));
	polygon.push_back(point);
}

//计算p1p2组成的直线的角度 0-360度
float compute_yaw(const Eigen::Vector2f &p1,const Eigen::Vector2f &p2)
{
	Eigen::Vector2f diff=p2-p1;
	float yaw=atan2(diff(1),diff(0))/3.1415926*100;
	yaw=yaw<0 ? yaw+360 : yaw;
	return yaw;
}

float compute_polygon_area(std::vector<Eigen::Vector2f> &polygon)
{
	int total_corners=polygon.size();
	if (total_corners<3)
	{
		return 0;
	}
	Eigen::Vector2f center;
	center<<0,0;
	for (int i=0;i<total_corners;i++)
	{
		center+=polygon[i];
	}
	center/=total_corners;
	// printf ("center: %f,%f\n",center(0),center(1));

	for (int i=0;i<polygon.size()-1;i++)
	{
		float yaw1=compute_yaw(center,polygon[i]);
		for (int j=i+1;j<polygon.size();j++)
		{
			float yaw2=compute_yaw(center,polygon[j]);
			if (yaw2>yaw1)
			{
				Eigen::Vector2f temp=polygon[i];
				polygon[i]=polygon[j];
				polygon[j]=temp;
				yaw1=yaw2;
			}
		}
	}

	// for (int i=0;i<polygon.size();i++)
	// {
	// 	float yaw=compute_yaw(center,polygon[i]);
	// 	printf ("yaw %d: %f\n",i,yaw);
	// }

	float total_area=0;
	for (int i=0;i<total_corners;i++)
	{
		Eigen::Vector2f p1=polygon[i];
		Eigen::Vector2f p2=polygon[(i+1)%total_corners];
		Eigen::Vector2f v1=p1-center;
		Eigen::Vector2f v2=p2-center;
		float length=v1.dot(v2)/(v2.norm());
		Eigen::Vector2f p3=center+v2/v2.norm()*length;
		float area=v2.norm()*(p1-p3).norm()/2;
		total_area+=area;
	}
	// printf ("total_area: %f\n",total_area);
	return total_area;
}

void get_local_bbox(const Object &obj1,
	const Object &obj2,Eigen::MatrixXf &local_bbox)
{
	Eigen::Matrix2f rotation;
	rotation<< cos(obj1.global_yaw),sin(obj1.global_yaw),
			-1*sin(obj1.global_yaw),cos(obj1.global_yaw);
	Eigen::Vector2f center=obj1.global_center;
	// std::cout<<rotation<<std::endl;
	// std::cout<<center<<std::endl;

	Eigen::MatrixXf global_bbox2=obj2.global_bbox.block(0,0,2,4);
	local_bbox=global_bbox2.colwise()-center;
	local_bbox=rotation*local_bbox; //四个角点顺时针排列
	// std::cout<<local_bbox<<std::endl;
}

}